import rclpy
from rclpy.node import Node
import serial
import time

class DogControlNode(Node):
    def __init__(self, uart_dev, baudrate):
        super().__init__('dog_control')
        self.get_logger().info("Successfully Login2.")
        
        # 初始化串口
        try:
            self.ser = serial.Serial(uart_dev, baudrate, timeout=1)
            self.get_logger().info(f"Serial port {uart_dev} opened with baudrate {baudrate}.")
        except Exception as e:
            self.get_logger().error(f"Failed to open serial port: {e}")
            raise
        
        # 初始化 flag 状态
        self.flag = 0  # 将 flag 定义为类的成员变量

        # 创建一个定时器，每隔1秒发送一次数据
        self.timer_send = self.create_timer(1.0, self.timer_send_callback)

        # 创建一个定时器，每隔0.1秒读取一次串口数据
        self.timer_receive = self.create_timer(0.1, self.timer_receive_callback)
        self.send_string("kup")

    def timer_send_callback(self):
        # 发送字符串数据
        if self.flag == 0:  # 使用 self.flag 访问成员变量
            #self.send_string("m8 45 12 -90 11 -45 15 -90 9 135 13 90 10 -135 14 90")
            self.send_string("kup")
            self.flag = 1  # 修改成员变量
        else:   
            #self.send_string("m8 0 12 0 11 0 15 0 9 0 13 0 10 0 14 0")
            self.send_string("kup")
            self.flag = 0  # 修改成员变量

    def timer_receive_callback(self):
        """
        定时读取串口接收到的数据并打印到终端。
        """
        try:
            # 检查串口缓冲区是否有数据
            if self.ser.in_waiting > 0:
                # 读取数据
                received_data = self.ser.read(self.ser.in_waiting)
                # 解码并打印接收到的数据到终端
                received_string = received_data.decode('utf-8', errors='ignore')
                print(f"Received: {received_string}")  # 打印到终端
        except Exception as e:
            print(f"Serial receive error: {e}")  # 打印错误到终端

    def send_string(self, string_to_send):
        """
        将字符串发送到下位机。
        """
        try:
            # 将字符串编码为字节流
            data = string_to_send.encode('utf-8')
            
            # 发送数据
            self.ser.write(data)
            self.get_logger().info(f"Sent string: {string_to_send}")
        except Exception as e:
            self.get_logger().error(f"Serial communication error: {e}")

    def __del__(self):
        # 关闭串口
        if hasattr(self, 'ser') and self.ser.is_open:
            self.ser.close()
            self.get_logger().info("Serial port closed.")

def main(args=None):
    rclpy.init(args=args)

    # 串口设备名称和波特率
    uart_dev = "/dev/ttyS1"  # 根据实际情况修改
    baudrate = 115200  # 根据实际情况修改

    # 创建节点实例
    node_dog_control = DogControlNode(uart_dev, baudrate)
    
    # 运行节点
    rclpy.spin(node_dog_control)
    
    # 关闭节点
    node_dog_control.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()